
#include <ros/ros.h>
#include <geographic_msgs/GeoPoseStamped.h>
#include <geographic_msgs/GeoPointStamped.h>
#include "drone_defs.h"

mavproxy_ros1::MavproxyState mavproxy_state;

void StateCallback(const mavproxy_ros1::MavproxyState::ConstPtr &msg)
{
    mavproxy_state = (*msg);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "gp_origin_publisher");
    ros::NodeHandle nh;

    ros::Subscriber mavproxy_state_sub = nh.subscribe<mavproxy_ros1::MavproxyState>(MAVPROXY_STATE, GLOBAL_DEFAULT_QUEUE_SIZE, StateCallback);
    ros::Publisher gp_origin_pub = nh.advertise<geographic_msgs::GeoPointStamped>("/mavros/global_position/gp_origin", GLOBAL_DEFAULT_QUEUE_SIZE);

    // 建立连接后等待3s
    ros::Rate rate(GLOBAL_COMMON_ROS_RATE / 2);
    ros::Time last_request = ros::Time::now();
    while (ros::ok() && !mavproxy_state.connected && ros::Time::now() - last_request < ros::Duration(DRONE_CONNECTION_TIMEOUT_DURATION))
    {
        ros::spinOnce();
        rate.sleep();
    }

    // 将当前GPS位置设置为全局原点
    geographic_msgs::GeoPointStamped gp_origin;
    gp_origin.header.stamp = ros::Time::now();
    gp_origin.header.frame_id = "local_origin";
    gp_origin.position.latitude = mavproxy_state.gps_position.latitude;
    gp_origin.position.longitude = mavproxy_state.gps_position.longitude;
    gp_origin.position.altitude = mavproxy_state.gps_position.altitude;

    // 循环发布origin 点
    while (ros::ok())
    {
        gp_origin_pub.publish(gp_origin);
        ros::spinOnce();
        rate.sleep();
    }
}
